/*
 * LR1.cpp
 *
 *  Created on: 11/12/2012
 *      Author: Julian Ovalle
 */

#include <cstdio>
#include <cstring>
#include <cmath>
#include <sys/time.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>

#include <cliMsgDef.h>
#include <MessageIO.h>
#include <Command.h>
#include <LR1.h>
#include <serializationUtils.h>

extern "C" {
#include <console_io.h>
}
MessageIO svc;

static void printMsg(Message msg) {

    printf("\n Msg:opts: %X ", msg.getOptions());
    printf("Type: %X ", msg.getType());
    printf("Len: %d ", msg.getLen());
    printf("nodeId: %s ", msg.getNodeId());
    printf("data: ");
    for (uint8_t i = 0; i < msg.getLen(); i++) {
        printf("%X", msg.getData()[i]);
    }
    printf("\n");
}

int avanzarDistanciaCM(uint8_t vel, unsigned short int dist, char * id,
        list<node_t> list) {

    Command com;

    com.setType(FWD_DIST);

    com.addByte(vel);

    com.addShort(dist);

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return (0);

}

int avanzarPulsos(uint8_t vel, short int pulsos, char * id, list<node_t> list) {

    Command com;

    com.setType(FWD_PULSE);

    com.addByte(vel);

    com.addShort(pulsos);

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return (0);

}

int girarGrados(uint8_t dir, uint8_t vel, short int grados, char *id,
        list<node_t> list) {

    Command com;

    com.setType(dir);

    com.addByte(vel);

    if (dir == RIGHT_DEG) {
        com.addShort(-grados);
    } else if (dir == LEFT_DEG) {
        com.addShort(grados);
    }

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return (0);

}

int girarRadianes(uint8_t dir, uint8_t vel, float radianes, char *id,
        list<node_t> list) {

    Command com;

    com.setType(dir);

    com.addByte(vel);

    if (dir == RIGHT_RAD) {
        com.addFloat(-(radianes));
    } else if (dir == LEFT_RAD) {
        com.addFloat(radianes);
    }

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return 0;

}

int avanzar(uint8_t vel, char *id, list<node_t> list) {

    Command com;

    com.setType(FWD);

    com.addByte(vel);

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return 0;

}

int retroceder(uint8_t vel, char *id, list<node_t> list) {

    Command com;

    com.setType(BWD);

    com.addByte(vel);

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    return 0;

}

int detenerse(char *id, list<node_t> list) {

    Command com;

    com.setType(STOP);

    com.setNodeId(id);
    com.setDestinationList(list);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }
    return 0;

}

unsigned short int* barridoUS(char *id, list<node_t> list) {

    Command com;

    com.setType(SWEEP);

    com.setNodeId(id);
    com.setDestinationList(list);
    com.setOptions(RESPONSE_REQUIRED_BIT);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return NULL;
    }

    uint16_t *ret = new uint16_t[10];
#ifdef LR1_DEBUG
    printMsg(*svc.getResponse());
#endif
    for (uint8_t i = 0; i < 10; i++) {
        ret[i] = deSerializeShort(&svc.getResponse()->getData()[2 * i]);
    }

    return ret;

}

short int sensorUSobtenerMedidaCM(char *id, list<node_t> list) {

    Command com;

    com.setType(READ);

    com.setNodeId(id);
    com.setDestinationList(list);
    com.setOptions(RESPONSE_REQUIRED_BIT);

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }

    unsigned short int val = -1;
    val = deSerializeShort(&svc.getResponse()->getData()[0]);

#ifdef LR1_DEBUG
    printMsg(*svc.getResponse());
    printf("\nobtenerUS %d\n", val);
#endif

    return val;
}

int testCommand(char *id, list<node_t> list) {

    Command com;

    com.setType(TEST);

    com.setNodeId(id);
    com.setDestinationList(list);
    com.setOptions(RESPONSE_REQUIRED_BIT);

    for (uint8_t i = 0; i < 24; i++) {
        com.addByte((rand() % (254 + 1 - 2)) + 2);
    }

    //printMsg(*com.toMessage());
    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }
//
#ifdef LR1_DEBUG
    printMsg(*svc.getResponse());
    //printf("\ntestData %s\n", svc.getResponse()->getData());
#endif

    if (strcmp((char*) com.getData(), (char*) svc.getResponse()->getData())
            != 0) {
        return -2;
    }

#ifdef LR1_DEBUG
        printf("\ntest data OK \n");
#endif
        return 0;

}

int testCommand2(char *id, list<node_t> list) {

    Command com;

    com.setType(TEST2);

    com.setNodeId(id);
    com.setDestinationList(list);

    for (uint8_t i = 0; i < 18; i++) {
        com.addByte((rand() % (254 + 1 - 2)) + 2);
    }

    if (svc.MessageOutput(com.toMessage(), com.getDestinationList()) == -1) {
#ifdef LR1_DEBUG
        perror("Send");
#endif
        return -1;
    }
    return 0;
}

void comander(MessageIO msg) {

    svc = msg;
    char input[20];
    char c = '0';
    char prev;
    char *id = input;
    list<node_t> b;
    node_t n;
    srand(time(NULL));

    uint8_t i = 0;

    printf("\nWho? (o 'b' para broadcast, 'm' para multicast): ");
    scanf("%s", input);

    if (strcmp(input, "m") == 0) {

        i = 0;
        printf("\nId de cada integrante del grupo (0 para finalizar).");

        while (n.id[0] != '0') {

            printf("\nId %d: ", i++);
            scanf("%s", n.id);
            if (strcmp(n.id, "0") != 0) {
                b.push_back(n);
            }
        }

        id = NULL;
        for (list<node_t>::iterator it = b.begin(); it != b.end(); it++) {
            printf("\nId ---> %s ", it->id);
        }

    } else if (strcmp(input, "b") == 0) {
        strcpy(id, "broadcast");
    }

    console_io_open();
    printf("\nESC para salir.\n ");
    //-- Main loop
    do {
        //-- if there is a key pressed by the user
        if (console_io_kbhit()) {

            //-- Read the character
            c = console_io_getch();

            //-- Send the character to the serial port, if different from ESC
            //if (c[0]!=ESC && c[0]!=prev) {
            if (c != ESC) {

                switch (c) {

                case FWD: {
                    avanzar(80, id, b);
                    break;
                }
                case FWD_DIST: {
                    avanzarDistanciaCM(80, 40, id, b);
                    break;
                }
                case FWD_PULSE: {
                    avanzarPulsos(80, 40, id, b);
                    break;
                }
                case RIGHT_DEG: {
                    girarGrados(RIGHT_DEG, 60, 30, id, b);
                    break;
                }
                case LEFT_DEG: {
                    girarGrados(LEFT_DEG, 60, 30, id, b);
                    break;
                }
                case RIGHT_RAD: {
                    girarRadianes(RIGHT_RAD, 40, 0.786000, id, b);
                    break;
                }
                case LEFT_RAD: {
                    girarRadianes(LEFT_RAD, 40, 0.786000, id, b);
                    break;
                }
                case BWD: {
                    retroceder(80, id, b);
                    break;
                }
                case STOP: {
                    detenerse(id, b);
                    break;
                }
                case SWEEP: {
                    uint16_t *ret = barridoUS(id, b);
                    if (ret != NULL) {
                        printf("\n\nlectura: ");
                        for (i = 0; i < 10; i++) {
                            printf("%d ", ret[i]);
                        }
                        printf("\n");
                    }
                    break;
                }
                case READ: {
                    int16_t ret = 0;
                    ret = sensorUSobtenerMedidaCM(id, b);
                    if (ret > 0) {
                        printf("\n\nlectura: %d\n", ret);
                    }
                    break;
                }
                case TEST: {
                    testCommand(id, b);
                    break;
                }
                case TEST2: {
                    testCommand2(id, b);
                    break;
                }
                case 0x75:
                    svc.updateNodeList();
                    break;

                default:
                    break;
                }
                prev = c;
                //usleep(100000);
            }
        }
    } while (c != ESC);

    console_io_close();

}

void commandTester(MessageIO msg) {

    svc = msg;
    char input[20];
    char c = '0';
    char *id = input;
    list<node_t> b;
    node_t n;
    srand(time(NULL));

    uint16_t i = 0;

    printf("\nTest Who? (o 'b' para broadcast, 'm' para multicast): ");
    scanf("%s", input);

    if (strcmp(input, "m") == 0) {

        i = 0;
        printf("\nId de cada integrante del grupo (0 para finalizar).");

        while (n.id[0] != '0') {

            printf("\nId %d: ", i++);
            scanf("%s", n.id);
            if (strcmp(n.id, "0") != 0) {
                b.push_back(n);
            }
        }

        id = NULL;
        for (list<node_t>::iterator it = b.begin(); it != b.end(); it++) {
            printf("\nId ---> %s ", it->id);
        }

    } else if (strcmp(input, "b") == 0) {
        strcpy(id, "broadcast");
    }

    unsigned int j = 0, good = 0, corrupt = 0, timeouts = 0;
    unsigned long iftime = 0;
    unsigned int sample_size = 1000;
    unsigned int num_samples = 50;
    unsigned int good_sum = 0, timeout_sum = 0, corrupt_sum = 0, timing_sum = 0, iftime_sum = 0,
            sec = 0;
    struct timeval timer;
    struct timeval frame_timer;

    unsigned int g[num_samples];
    unsigned int touts[num_samples];
    unsigned int corr[num_samples];
    unsigned int times[num_samples];
    unsigned int iftimes[num_samples];

    int r = 0;
    unsigned long usec = 0;

    FILE *fp = fopen("tests_uni2.txt", "a+");

    //uint8_t num_tests=4;

    fprintf(fp, "#UNIDIRECTIONAL 16-4.18 byte frames. sample size: %d, num_samples: %d\n", sample_size,
            num_samples);
    fprintf(fp,
            "#columns: good: avg, s ; corrupt: avg, s ; timeouts: avg, s ; time taken: avg, s");
    fprintf(fp, " ; timeout in ms\n");

    //for (uint8_t h = 0; h < num_tests; h++) {

        for (i = 0; i < num_samples; i++) {

            gettimeofday(&timer, NULL);
            sec = timer.tv_sec;

            for (j = 0; j < sample_size; j++) {

                gettimeofday(&frame_timer, NULL);
                usec = frame_timer.tv_usec;
                //r = testCommand(id, b);
                r = testCommand2(id, b);

                if (r == 0) {
                    good++;
                    //printf("\nOK\n");

                } else if (r == -1) {
                    //printf("\nTIMEOUT\n");
                    timeouts++;

                } else if (r == -2) {
                    //printf("\nCORRUPT\n");
                    corrupt++;
                }

                gettimeofday(&frame_timer, NULL);
                usec = frame_timer.tv_usec - usec;

                iftime+=usec;
//
//            switch (r) {
//                case 0: {
//                    good++;
//                    printf("\nOK\n");
//                    break; }
//                case (-1): {
//                    printf("\nTIMEOUT\n");
//                    timeouts++;
//                    break;}
//                case (-2):{
//                    printf("\nCORRUPT\n");
//                    corrupt++;
//                    break;}
//                default:
//                    break;
//            }

            }
            gettimeofday(&timer, NULL);
            sec = timer.tv_sec - sec;
            printf("\n%d-elapsed time: %d sec", i, sec);

            fprintf(fp, "\t %d \t %d \t %d \t %d \t %d\n", good, corrupt,timeouts,sec, (iftime/sample_size));

            timing_sum += sec;
            good_sum += good;
            corrupt_sum += corrupt;
            timeout_sum += timeouts;
            //iftime_sum += (iftime/sample_size);

            g[i] = good;
            corr[i] = corrupt;
            touts[i] = timeouts;
            times[i] = sec;

            good = 0;
            corrupt = 0;
            timeouts = 0;
            iftime = 0;
            memset(&timer, 0, sizeof(struct timeval));

        }
        double good_avg = good_sum / num_samples;
        double corrupt_avg = corrupt_sum / num_samples;
        double timeout_avg = timeout_sum / num_samples;
        double timing_avg = timing_sum / num_samples;

        good_sum = 0;
        timeout_sum = 0;
        corrupt_sum = 0;
        timing_sum = 0;

        for (i = 0; i < num_samples; i++) {
            good_sum += pow((g[i] - good_avg), 2);
            timeout_sum += pow((touts[i] - timeout_avg), 2);
            corrupt_sum += pow((corr[i] - corrupt_avg), 2);
            timing_sum += pow((times[i] - timing_avg), 2);
        }

        double good_std = sqrt(good_sum / (num_samples - 1));
        double corrupt_std = sqrt(corrupt_sum / (num_samples - 1));
        double timeout_std = sqrt(timeout_sum / (num_samples - 1));
        double timing_std = sqrt(timing_sum / (num_samples - 1));

        fprintf(fp, "%f  %f ", good_avg, good_std);
        fprintf(fp, " %f  %f ", corrupt_avg, corrupt_std);
        fprintf(fp, " %f  %f ", timeout_avg, timeout_std);
        fprintf(fp, " %f  %f \n", timing_avg, timing_std);
        //fprintf(fp, " %d\n", rx_timeout_ms);

        good_sum = 0;
        corrupt_sum = 0;
        timeout_sum = 0;
        timing_sum = 0;
    //}
    fclose(fp);

}
